/******************************************************************************
 *  Nano-RK, a real-time operating system for sensor networks.
 *  Copyright (C) 2007, Real-Time and Multimedia Lab, Carnegie Mellon University
 *  All rights reserved.
 *
 *  This is the Open Source Version of Nano-RK included as part of a Dual
 *  Licensing Model. If you are unsure which license to use please refer to:
 *  http://www.nanork.org/nano-RK/wiki/Licensing
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, version 2.0 of the License.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 *  Contributing Authors (specific to this file):
 *  Zane Starr
 *******************************************************************************/


#include <nrk.h>
#include <include.h>
#include <ulib.h>
#include <stdio.h>
#include <avr/sleep.h>
#include <hal.h>
#include <nrk_error.h>
#include <nrk_timer.h>
#include <nrk_driver_list.h>
#include <nrk_driver.h>
#include <ff_basic_sensor.h>

NRK_STK Stack1[NRK_APP_STACKSIZE];
nrk_task_type TaskOne;
void Task1(void);

#define DEVIATION 10
uint16_t z_baseline, x_baseline, y_baseline;

void nrk_create_taskset();
void nrk_register_drivers();
uint8_t kill_stack(uint8_t val);

int
main ()
{
	uint8_t t;
	nrk_setup_ports();
	nrk_setup_uart(UART_BAUDRATE_115K2);
	
	
	printf( PSTR("starting...\r\n") );
	
	nrk_init();
	nrk_time_set(0,0);
	
	nrk_register_drivers();
	nrk_create_taskset ();
	nrk_start();
	
	return 0;
}


void Task1()
{
	uint16_t cnt, calibration;
	int8_t i,fd,val;
	uint16_t x_val, y_val, z_val;
	
	printf( "My node's address is %d\r\n",NODE_ADDR );
	
	printf( "Task1 PID=%d\r\n",nrk_get_pid());
	x_baseline = 0;
	y_baseline = 0;
	z_baseline = 0;
	cnt=0;
	calibration = 1;
	while(1) {
		// Open ADC device as read 
		fd=nrk_open(FIREFLY_SENSOR_BASIC,READ);
		if(fd==NRK_ERROR) nrk_kprintf(PSTR("Failed to open sensor driver\r\n"));
		nrk_led_toggle(BLUE_LED);
		
		/* read x value */
		val=nrk_set_status(fd,SENSOR_SELECT,ACC_X);
		val=nrk_read(fd,&x_val,2);
		
		/* read y value */
		val=nrk_set_status(fd,SENSOR_SELECT,ACC_Y);
		val=nrk_read(fd,&y_val,2);
		
		/* read z value  */
		val=nrk_set_status(fd,SENSOR_SELECT,ACC_Z);
		val=nrk_read(fd,&z_val,2);
		
		if(cnt > 10){
			calibration = 0;
			printf( "z_baseline=%d\r\n",z_baseline);
			printf( "x_baseline=%d\r\n",x_baseline);
			printf( "y_baseline=%d\r\n",y_baseline);
			cnt = 0;
		}
		/* calibration */
		if(calibration > 0){			
			if(cnt == 0){
				x_baseline =  x_val;
				y_baseline =  y_val;
				z_baseline =  z_val;
			}
			else{
				x_baseline = (x_baseline + x_val)>>1;
				y_baseline = (y_baseline + y_val)>>1;
				z_baseline = (z_baseline + z_val)>>1;
			}
			
			cnt++;
		}
		
		else{	
			
			/* 
			 * change in z value beyond threshold indicates cursor move
			 * determine cursor direction only on this indicator
			 */
			if((z_val > z_baseline + DEVIATION) || z_val < z_baseline - DEVIATION){
				
				printf( "acc_z=%d",z_val);
				printf( " acc_x=%d",x_val);
				printf( " acc_y=%d\r\n",y_val);
				if((x_val > (x_baseline + DEVIATION)) || (x_val  < (x_baseline - DEVIATION))){
					if(x_val > x_baseline) nrk_kprintf( PSTR("move down\r\n") );
					else nrk_kprintf( PSTR("move up\r\n") );
				}
				if((y_val > (y_baseline + DEVIATION)) || (y_val  < (y_baseline - DEVIATION))){
					if(y_val > y_baseline) nrk_kprintf( PSTR("move right\r\n") );
					else nrk_kprintf( PSTR("move left\r\n") );
				}
			}
			else {
				nrk_kprintf( PSTR("no move\r\n") );
			}
		}
		
		nrk_close(fd);
		nrk_wait_until_next_period();
		
	}
}


void
nrk_create_taskset()
{
	TaskOne.task = Task1;
	nrk_task_set_stk( &TaskOne, Stack1, NRK_APP_STACKSIZE);
	TaskOne.prio = 1;
	TaskOne.FirstActivation = TRUE;
	TaskOne.Type = BASIC_TASK;
	TaskOne.SchType = PREEMPTIVE;
	TaskOne.period.secs = 1;
	TaskOne.period.nano_secs = 100*NANOS_PER_MS; //*NANOS_PER_MS;
	TaskOne.cpu_reserve.secs = 1;
	TaskOne.cpu_reserve.nano_secs =  50*NANOS_PER_MS;
	TaskOne.offset.secs = 0;
	TaskOne.offset.nano_secs= 0;
	nrk_activate_task (&TaskOne);
	
}

void nrk_register_drivers()
{
	int8_t val;
	
	// Register the Basic FireFly Sensor device driver
	// Make sure to add: 
	//     #define NRK_MAX_DRIVER_CNT  
	//     in nrk_cfg.h
	// Make sure to add: 
	//     SRC += $(ROOT_DIR)/src/drivers/platform/$(PLATFORM_TYPE)/source/ff_basic_sensor.c
	//     in makefile
	val=nrk_register_driver( &dev_manager_ff_sensors,FIREFLY_SENSOR_BASIC);
	if(val==NRK_ERROR) nrk_kprintf( PSTR("Failed to load my ADC driver\r\n") );
	
}


